#pragma once
#include <bits/stdc++.h>
#include <cmath>
#include <mutex>
#include <string>
#include <thread>
#include <unistd.h>

namespace Tools {
static inline float normalize_angle( float t ) {
    const float result = fmod( t + M_PI, 2.0 * M_PI );
    if ( result <= 0.0 )
        return result + M_PI;
    return result - M_PI;
}
};  // namespace Tools

struct traj {
    // double time;
    std::string id;
    double      x;
    double      y;
    double      phi;
    double      vel;
    double      delta;
    double      kappa;
};

template < typename T > class safe_class {
public:
    void set( const T &data ) {
        std::lock_guard< std::mutex > lock( mutex_ );
        ndata.store( true );
        m_data = data;
    }
    bool check() {
        return ndata.load();
    }

    T get() {
        std::lock_guard< std::mutex > lock( mutex_ );
        ndata.store( false );
        return m_data;
    }

private:
    T                   m_data;
    std::mutex          mutex_;
    std::atomic< bool > ndata;
};